// Copyright (C) 2023 Thies Lennart Alff
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301,
// USA

#include <eigen3/Eigen/Dense>
#include <hippo_common/convert.hpp>
#include <hippo_common/param_utils.hpp>
#include <hippo_control_msgs/msg/actuator_setpoint.hpp>
#include <hippo_control_msgs/msg/attitude_target.hpp>
#include <hippo_control_msgs/msg/thrust_attitude_target.hpp> //新增
#include <nav_msgs/msg/odometry.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>



// 命名空间的作用：在代码里可以直接写 GeometricController 而不是 hippo_control::attitude_control::GeometricController
using namespace hippo_common;
using namespace hippo_control_msgs::msg;
using namespace rcl_interfaces::msg;
using namespace geometry_msgs::msg;
using nav_msgs::msg::Odometry;
using std::placeholders::_1;



//需要添加命名空间才能找到组件
namespace hippo_control {
namespace nodes {  // 添加 nodes 命名空间

class PoseControlBlueROVNode : public rclcpp::Node {
 public:
  PoseControlBlueROVNode(const rclcpp::NodeOptions &options)
  : Node("pose_controller", options){

    RCLCPP_INFO(get_logger(), "Declaring parameters.");
    DeclareParams();
    InitPublishers();
    InitTimers();
    // DelaySub();
    InitSubscriptions();
  }

  // 在成员函数中，获取函数指针或者变量指针，需要在前面加上类名（即使在本类中）
  void InitTimers() {
    setpoint_timeout_timer_ = rclcpp::create_timer(
        this, get_clock(), std::chrono::milliseconds(2000),
        std::bind(&PoseControlBlueROVNode::OnSetpointTimeout, this));
  }
  // 修改 DeclareParams 方法
  void DeclareParams() {

     // 先声明参数（带默认值），确保参数存在
    declare_parameter("position_gain", 1.0);
    declare_parameter("velocity_gain", 1.0);
    
    declare_parameter("gains.roll.p", 1.0);
    declare_parameter("gains.roll.i", 0.0);
    declare_parameter("gains.roll.d", 0.0);
    
    declare_parameter("gains.pitch.p", 1.0);
    declare_parameter("gains.pitch.i", 0.0);
    declare_parameter("gains.pitch.d", 0.0);
    
    declare_parameter("gains.yaw.p", 1.0);
    declare_parameter("gains.yaw.i", 0.0);
    declare_parameter("gains.yaw.d", 0.0);

    // 然后读取参数值（会覆盖默认值为配置文件中的值）
    // 读取位置和速度增益
    position_gain_ = get_parameter("position_gain").as_double();
    velocity_gain_ = get_parameter("velocity_gain").as_double();

    // 读取滚转增益
    torque_p_gains_.x() = get_parameter("gains.roll.p").as_double();
    torque_i_gains_.x() = get_parameter("gains.roll.i").as_double();
    torque_d_gains_.x() = get_parameter("gains.roll.d").as_double();
    
    // 读取俯仰增益
    torque_p_gains_.y() = get_parameter("gains.pitch.p").as_double();
    torque_i_gains_.y() = get_parameter("gains.pitch.i").as_double();
    torque_d_gains_.y() = get_parameter("gains.pitch.d").as_double();
    
    // 读取偏航增益
    torque_p_gains_.z() = get_parameter("gains.yaw.p").as_double();
    torque_i_gains_.z() = get_parameter("gains.yaw.i").as_double();
    torque_d_gains_.z() = get_parameter("gains.yaw.d").as_double();

    RCLCPP_INFO(get_logger(), "Parameters loaded:");
    RCLCPP_INFO(get_logger(), "  position_gain: %.2f", position_gain_);
    RCLCPP_INFO(get_logger(), "  velocity_gain: %.2f", velocity_gain_);
    RCLCPP_INFO(get_logger(), "  Roll gains - P:%.2f, I:%.2f, D:%.2f", 
                torque_p_gains_.x(), torque_i_gains_.x(), torque_d_gains_.x());
    RCLCPP_INFO(get_logger(), "  Pitch gains - P:%.2f, I:%.2f, D:%.2f", 
                torque_p_gains_.y(), torque_i_gains_.y(), torque_d_gains_.y());
    RCLCPP_INFO(get_logger(), "  Yaw gains - P:%.2f, I:%.2f, D:%.2f", 
                torque_p_gains_.z(), torque_i_gains_.z(), torque_d_gains_.z());

  }

  void DelaySub() {
    // 定时器每 10ms 触发一次
    rclcpp::TimerBase::SharedPtr delay_timer_ = rclcpp::create_timer(
        this, get_clock(), std::chrono::milliseconds(10),
        std::bind(&PoseControlBlueROVNode::CheckAndPublishDelayedControl, this));
  } 



  void InitPublishers() {
    std::string topic;
    rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();

    topic = "thrust_setpoint";
    thrust_pub_ = create_publisher<hippo_control_msgs::msg::ActuatorSetpoint>(
        topic, rclcpp::SensorDataQoS());

    topic = "torque_setpoint";
    torque_pub_ = create_publisher<hippo_control_msgs::msg::ActuatorSetpoint>(
        topic, rclcpp::SensorDataQoS());

    topic = "~/current_setpoint";
    setpoint_pub_ = create_publisher<ThrustAttitudeTarget>(topic, qos);
  }

  void InitSubscriptions() {
    std::string topic;
    rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();

    topic = "attitude_target_desired";
    attitude_target_desired_sub_ = create_subscription<geometry_msgs::msg::QuaternionStamped>(
        topic, 10, [this](const geometry_msgs::msg::QuaternionStamped::SharedPtr _msg) {
          OnAttitudeTargetDesired(_msg);
        });
    
    topic = "position_target";
    // position_target_赋值
    position_target_sub_ = create_subscription<geometry_msgs::msg::PointStamped>(
        topic, 10,
        [this](const geometry_msgs::msg::PointStamped::SharedPtr _msg) {
          OnPositionTarget(_msg);
        });

    topic = "velocity_target";
    // velocity_target_赋值
    velocity_target_sub_ =
        create_subscription<geometry_msgs::msg::Vector3Stamped>(
            topic, 10,
            [this](const geometry_msgs::msg::Vector3Stamped::SharedPtr _msg) {
              OnVelocityTarget(_msg);
            });

    topic = "odometry";
    odometry_sub_ = create_subscription<Odometry>(
        topic, qos, std::bind(&PoseControlBlueROVNode::OnOdometry, this, _1));
  }

  hippo_control_msgs::msg::ActuatorSetpoint ZeroMsg(rclcpp::Time _stamp) {
    hippo_control_msgs::msg::ActuatorSetpoint msg;
    msg.header.stamp = _stamp;
    msg.x = 0.0;
    msg.y = 0.0;
    msg.z = 0.0;
    return msg;
  }

  void OnSetpointTimeout() {
    if (setpoint_timed_out_) {
      return;
    }
    RCLCPP_WARN(get_logger(), "Setpoint timed out. Sending zero commands.");
    setpoint_timed_out_ = false; //禁用超时
    auto thrust_msg = ZeroMsg(now());
    auto torque_msg = ZeroMsg(now());
    thrust_pub_->publish(thrust_msg);
    torque_pub_->publish(torque_msg);
  }

  // 新增：目标姿态回调函数
  void OnAttitudeTargetDesired(
      const geometry_msgs::msg::QuaternionStamped::SharedPtr _msg) {
    hippo_common::convert::RosToEigen(_msg->quaternion, attitude_target_);
    received_attitude_target_ = true;  // 添加标志位
  }

  void OnPositionTarget(
      const geometry_msgs::msg::PointStamped::SharedPtr _msg) {
    hippo_common::convert::RosToEigen(_msg->point, position_target_);
    received_position_target_ = true;  // 添加标志位
  }

  void OnVelocityTarget(
      const geometry_msgs::msg::Vector3Stamped::SharedPtr _msg) {
    hippo_common::convert::RosToEigen(_msg->vector, velocity_target_);
  }


  // 新增修改
  void OnOdometry(const Odometry::SharedPtr _msg) {

    // 添加状态检查
    if (received_attitude_target_ == false && received_position_target_ == false) {
      RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 2000, 
                          "Waiting for position and attitude targets");
      return;  // 如果没有收到必要的数据，直接返回
    }
    
    
    //当前位置
    Eigen::Vector3d position_{0.0, 0.0, 0.0};
    hippo_common::convert::RosToEigen(_msg->pose.pose.position, position_);
    //当前速度
    Eigen::Vector3d velocity_{0.0, 0.0, 0.0};
    hippo_common::convert::RosToEigen(_msg->twist.twist.linear, velocity_);
    //当前姿态
    Eigen::Quaterniond attitude_{1.0, 0.0, 0.0, 0.0};
    hippo_common::convert::RosToEigen(_msg->pose.pose.orientation, attitude_);

    RCLCPP_INFO(get_logger(), "OnOdometry:@@@@@@@@@@@@@@@@ - X: %.3f, Y: %.3f, Z: %.3f", position_[0], position_[1], position_[2]);
    //当前角速度
    const auto &omega = _msg->twist.twist.angular;
    Eigen::Vector3d v_angular{omega.x, omega.y, omega.z};

    rclcpp::Time t_now = now();

    // thrust
    // 修改：计算推力向量（在世界坐标系下）
    Eigen::Vector3d thrust_world = Update_thrust(position_, velocity_);
    // 将世界坐标系的推力转换到机体坐标系
    Eigen::Vector3d thrust_body = attitude_.inverse() * thrust_world;

    RCLCPP_INFO(get_logger(), "3333 Time : %.3f", t_now.seconds());

    RCLCPP_INFO(get_logger(), "thrust_body - X: %.3f, Y: %.3f, Z: %.3f", thrust_body[0], thrust_body[1], thrust_body[2]);

    // torque
    Eigen::Vector3d torque = Update_torque(attitude_, v_angular);
    RCLCPP_INFO(get_logger(), "torque - X: %.3f, Y: %.3f, Z: %.3f", torque[0], torque[1], torque[2]);


    // 修改：发布目标姿态和推力向量
    PublishPoseTarget(torque, thrust_body);


    // 将控制信号和时间戳存入队列
    {
      std::lock_guard<std::mutex> lock(mutex_);
      thrust_queue_.emplace_back(thrust_body, t_now);
      torque_queue_.emplace_back(torque, t_now);
    }


  }


  // 修改：Update函数现在只计算推力，返回世界坐标系下的推力向量
  Eigen::Vector3d Update_thrust(
      const Eigen::Vector3d &position_cur, const Eigen::Vector3d &velocity_cur) {
    
    //计算推力
    Eigen::Vector3d thrust_result = (position_target_ - position_cur) * position_gain_ +
                          (velocity_target_ - velocity_cur) * velocity_gain_ ;
    return thrust_result; 
  }

  Eigen::Vector3d Update_torque(
      const Eigen::Quaterniond &attitude_cur, const Eigen::Vector3d &v_angular_cur) {
    
    // 当前姿态的四元数规范化
    static Eigen::Quaterniond orientation_canonical{1.0, 0.0, 0.0, 0.0};
    if (orientation_canonical.dot(attitude_cur) < 0) {
      orientation_canonical.w() = -1.0 * attitude_cur.w();
      orientation_canonical.x() = -1.0 * attitude_cur.x();
      orientation_canonical.y() = -1.0 * attitude_cur.y();
      orientation_canonical.z() = -1.0 * attitude_cur.z();
    } else {
      orientation_canonical = attitude_cur;
    }
    // 当前姿态矩阵
    Eigen::Matrix3d R = orientation_canonical.toRotationMatrix();
    // 目标姿态矩阵
    Eigen::Matrix3d R_desired = attitude_target_.toRotationMatrix();
    //结果 R_error 是反对称矩阵
    Eigen::Matrix3d R_error =
        0.5 * (R_desired.transpose() * R - R.transpose() * R_desired);

    // 实际上提取的是：[a1, a2, a3]，是各个旋转角的弧度误差
    Eigen::Array3d R_error_vector{R_error(1, 2), R_error(2, 0), R_error(0, 1)};

    Eigen::Array3d angular_velocity_error{v_angular_target_ - v_angular_cur};

    Eigen::Array3d torque;
    torque = torque_p_gains_ * R_error_vector + torque_d_gains_ * angular_velocity_error;

    return Eigen::Vector3d{torque};
  }

  void PublishPoseTarget(
      const Eigen::Vector3d &torque, const Eigen::Vector3d &thrust_body) {
    if (thrust_pub_ == nullptr || torque_pub_ == nullptr) {
      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
                          "Publisher for thrust_pub_ and torque_pub_ not available");
      return;
    }
    
    hippo_control_msgs::msg::ActuatorSetpoint thrust_msg;
    hippo_control_msgs::msg::ActuatorSetpoint torque_msg;
    {
      std::lock_guard<std::mutex> lock(mutex_);
      if (setpoint_timed_out_) {
        thrust_msg = ZeroMsg(now());
        torque_msg = ZeroMsg(now());
        RCLCPP_INFO(get_logger(), "setpoint_timed_out_ ");
      } else {
        thrust_msg.header.stamp = now();
        torque_msg.header.stamp = thrust_msg.header.stamp;
        
        thrust_msg.x = thrust_body.x();  // x方向推力
        thrust_msg.y = thrust_body.y();  // y方向推力
        thrust_msg.z = thrust_body.z();  // z方向推力

        torque_msg.x = torque.x();
        torque_msg.y = torque.y();
        torque_msg.z = torque.z();
      }
    }
    thrust_pub_->publish(thrust_msg);
    torque_pub_->publish(torque_msg);

  }


void CheckAndPublishDelayedControl() {
    std::lock_guard<std::mutex> lock(mutex_);

    rclcpp::Time now_time = now();

    // 修改：遍历所有队列元素，发布所有已经延迟超过2秒的数据
    auto process_queue = [&](std::deque<ControlData>& queue, 
                            rclcpp::Publisher<ActuatorSetpoint>::SharedPtr publisher) {
      auto it = queue.begin();
      while (it != queue.end()) {
        if ((now_time - it->timestamp).seconds() >= 2.0) {
          // 发布数据
          ActuatorSetpoint msg;
          msg.x = it->data.x();
          msg.y = it->data.y();
          msg.z = it->data.z();
          msg.header.stamp = it->timestamp;  // 使用原始时间戳
          publisher->publish(msg);
          
          // 移除已发布的数据
          it = queue.erase(it);
        } else {
          // 如果当前数据还未到2秒，后续的数据时间戳更大，所以退出循环
          break;
        }
      }
    };

    // 处理推力队列和转矩队列
    process_queue(thrust_queue_, thrust_pub_);
    process_queue(torque_queue_, torque_pub_);
  }



 private:
  std::mutex mutex_;

  // 控制信号队列，存储推力和转矩及其时间戳
  struct ControlData {
    Eigen::Vector3d data;
    rclcpp::Time timestamp;

    ControlData(const Eigen::Vector3d& data_, rclcpp::Time timestamp_)
        : data(data_), timestamp(timestamp_) {}
  };
  std::deque<ControlData> thrust_queue_;
  std::deque<ControlData> torque_queue_;

  //////////////////////////////////////////////////////////////////////////////
  // ros params
  //////////////////////////////////////////////////////////////////////////////
  double gain_roll_p_;
  double gain_roll_d_;
  double gain_pitch_p_;
  double gain_pitch_d_;
  double gain_yaw_p_;
  double gain_yaw_d_;

  // alias to use template instance with default allocator

  bool setpoint_timed_out_{false};

  Eigen::Quaterniond attitude_target_{1.0, 0.0, 0.0, 0.0};
  Eigen::Vector3d v_angular_target_{0.0, 0.0, 0.0};
  Eigen::Vector3d position_target_{0.0, 0.0, 0.0};
  Eigen::Vector3d velocity_target_{0.0, 0.0, 0.0};

  bool received_attitude_target_{false};
  bool received_position_target_{false};

  Eigen::Array3d torque_p_gains_{1.0, 1.0, 1.0};
  Eigen::Array3d torque_i_gains_{0.1, 0.1, 0.1};
  Eigen::Array3d torque_d_gains_{0.1, 0.1, 0.1};
  double position_gain_{1.0};
  double velocity_gain_{1.0};


  rclcpp::TimerBase::SharedPtr setpoint_timeout_timer_;

  //////////////////////////////////////////////////////////////////////////////
  // publishers
  //////////////////////////////////////////////////////////////////////////////
  rclcpp::Publisher<hippo_control_msgs::msg::ActuatorSetpoint>::SharedPtr
      thrust_pub_{nullptr};
  rclcpp::Publisher<hippo_control_msgs::msg::ActuatorSetpoint>::SharedPtr
      torque_pub_{nullptr};
  rclcpp::Publisher<ThrustAttitudeTarget>::SharedPtr 
      setpoint_pub_{nullptr};

  //////////////////////////////////////////////////////////////////////////////
  // subscriptions
  //////////////////////////////////////////////////////////////////////////////
  rclcpp::Subscription<geometry_msgs::msg::QuaternionStamped>::SharedPtr 
      attitude_target_desired_sub_{nullptr};
  rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr
      velocity_target_sub_{nullptr};
  rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr
      position_target_sub_{nullptr};
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr 
      odometry_sub_{nullptr};

  OnSetParametersCallbackHandle::SharedPtr p_gains_cb_handle_;
  OnSetParametersCallbackHandle::SharedPtr d_gains_cb_handle_;
};

}  // namespace nodes
}  // namespace hippo_control





#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(hippo_control::nodes::PoseControlBlueROVNode)